import { Agent } from "./Agent";
import { KdTree } from "./KdTree";
import { Line } from "./Line";
import { Obstacle } from "./Obstacle";
import { Vector2 } from "./Vector2";


export class Simulator {


    defaultAgent_: Agent;
    agents_: Agent[];

    time_: number = 0;
    timeStep_: number;

    obstacles_: Obstacle[];
    kdTree_: KdTree;

    static _instance: Simulator;


    constructor() {

        this.agents_ = [];
        this.obstacles_ = [];
        this.time_ = 0;
        this.defaultAgent_ = null;
        this.kdTree_ = new KdTree();
        this.timeStep_ = 1;

        Simulator._instance = this;
    }

    public static Instance(): Simulator {

        return Simulator._instance;
    }

    public addAgent(position: Vector2) {
        if (this.defaultAgent_ == null) {
            return -1;
        }

        var agent: Agent = new Agent();

        agent.position_ = position;
        agent.maxNeighbors_ = this.defaultAgent_.maxNeighbors_;
        agent.maxSpeed_ = this.defaultAgent_.maxSpeed_;
        agent.neighborDist_ = this.defaultAgent_.neighborDist_;
        agent.radius_ = this.defaultAgent_.radius_;
        agent.timeHorizon_ = this.defaultAgent_.timeHorizon_;
        agent.timeHorizonObst_ = this.defaultAgent_.timeHorizonObst_;
        agent.velocity_ = this.defaultAgent_.velocity_;

        agent.id_ = this.agents_.length;

        this.agents_.push(agent);

        return this.agents_.length - 1;

    }

    public addObstacle(vertices: Vector2[]): number {

        if (vertices.length < 2) {
            return -1;
        }

        var obstacleNo = this.obstacles_.length;

        for (var i = 0; i < vertices.length; ++i) {
            var obstacle: Obstacle = new Obstacle();
            obstacle.point_ = vertices[i];

            if (i != 0) {
                obstacle.prevObstacle_ = this.obstacles_[this.obstacles_.length - 1];
                obstacle.prevObstacle_.nextObstacle_ = obstacle;
            }

            if (i == vertices.length - 1) {
                obstacle.nextObstacle_ = this.obstacles_[obstacleNo];
                obstacle.nextObstacle_.prevObstacle_ = obstacle;
            }

            obstacle.unitDir_ = Vector2.normalize(vertices[(i == vertices.length - 1 ? 0 : i + 1)].moins(vertices[i]));

            if (vertices.length == 2) {
                obstacle.isConvex_ = true;
            }
            else {
                obstacle.isConvex_ = (Vector2.leftOf(vertices[(i == 0 ? vertices.length - 1 : i - 1)], vertices[i], vertices[(i == vertices.length - 1 ? 0 : i + 1)]) >= 0);
            }

            obstacle.id_ = this.obstacles_.length;

            this.obstacles_.push(obstacle);
        }

        return obstacleNo;
    }

    public doStep() {

        this.kdTree_.buildAgentTree();

        for (var i = 0; i < this.getNumAgents(); ++i) {
            this.agents_[i].computeNeighbors();
            this.agents_[i].computeNewVelocity();
        }
        for (var i = 0; i < this.getNumAgents(); ++i) {
            this.agents_[i].update();
        }

        this.time_ += this.timeStep_;
    }

    public getAgentAgentNeighbor(agentNo: number, neighborNo: number) {
        return (this.agents_[agentNo].agentNeighbors_[neighborNo].Value as Agent).id_;
    }

    /**
         * \brief      Returns the maximum neighbor count of a specified agent.
         * \param      agentNo         The number of the agent whose maximum
         *                             neighbor count is to be retrieved.
         * \return     The present maximum neighbor count of the agent.
         */
    public getAgentMaxNeighbors(agentNo: number) {
        return this.agents_[agentNo].maxNeighbors_;
    }

    /**
         * \brief      Returns the maximum speed of a specified agent.
         * \param      agentNo         The number of the agent whose maximum speed
         *                             is to be retrieved.
         * \return     The present maximum speed of the agent.
         */
    public getAgentMaxSpeed(agentNo: number) {
        return this.agents_[agentNo].maxSpeed_;
    }

    /**
         * \brief      Returns the maximum neighbor distance of a specified
         *             agent.
         * \param      agentNo         The number of the agent whose maximum
         *                             neighbor distance is to be retrieved.
         * \return     The present maximum neighbor distance of the agent.
         */
    public getAgentNeighborDist(agentNo: number) {
        return this.agents_[agentNo].neighborDist_;
    }

    /**
         * \brief      Returns the count of agent neighbors taken into account to
         *             compute the current velocity for the specified agent.
         * \param      agentNo         The number of the agent whose count of agent
         *                             neighbors is to be retrieved.
         * \return     The count of agent neighbors taken into account to compute
         *             the current velocity for the specified agent.
         */

    public getAgentNumAgentNeighbors(agentNo: number) {
        return this.agents_[agentNo].agentNeighbors_.length;
    }

    /**
         * \brief      Returns the count of obstacle neighbors taken into account
         *             to compute the current velocity for the specified agent.
         * \param      agentNo         The number of the agent whose count of
         *                             obstacle neighbors is to be retrieved.
         * \return     The count of obstacle neighbors taken into account to
         *             compute the current velocity for the specified agent.
         */
    public getAgentNumObstacleNeighbors(agentNo: number) {
        return this.agents_[agentNo].obstacleNeighbors_.length;
    }

    /**
         * \brief      Returns the count of ORCA constraints used to compute
         *             the current velocity for the specified agent.
         * \param      agentNo         The number of the agent whose count of ORCA
         *                             constraints is to be retrieved.
         * \return     The count of ORCA constraints used to compute the current
         *             velocity for the specified agent.
         */
    public getAgentNumORCALines(agentNo: number) {
        return this.agents_[agentNo].orcaLines_.length;
    }

    /**
     * \brief      Returns the specified obstacle neighbor of the specified
     *             agent.
     * \param      agentNo         The number of the agent whose obstacle
     *                             neighbor is to be retrieved.
     * \param      neighborNo      The number of the obstacle neighbor to be
     *                             retrieved.
     * \return     The number of the first vertex of the neighboring obstacle
     *             edge.
     */
    public getAgentObstacleNeighbor(agentNo: number, neighborNo: number) {
        return <Agent>(this.agents_[agentNo].obstacleNeighbors_[neighborNo].Value).id_;
    }

    public getAgentPosition(agentNo: number): Vector2 {
        return this.agents_[agentNo].position_;
    }

    /**
         * \brief      Returns the two-dimensional preferred velocity of a
         *             specified agent.
         * \param      agentNo         The number of the agent whose
         *                             two-dimensional preferred velocity is to be
         *                             retrieved.
         * \return     The present two-dimensional preferred velocity of the agent.
         */
    public getAgentPrefVelocity(agentNo: number): Vector2 {
        return this.agents_[agentNo].prefVelocity_;
    }

    public getAgentRadius(agentNo: number): number {
        return this.agents_[agentNo].radius_;
    }

    /**
     * \brief      Returns the time horizon of a specified agent.
     * \param      agentNo         The number of the agent whose time horizon
     *                             is to be retrieved.
     * \return     The present time horizon of the agent.
     */
    public getAgentTimeHorizon(agentNo: number) {
        return this.agents_[agentNo].timeHorizon_;
    }

    /**
     * \brief      Returns the time horizon with respect to obstacles of a
     *             specified agent.
     * \param      agentNo         The number of the agent whose time horizon
     *                             with respect to obstacles is to be
     *                             retrieved.
     * \return     The present time horizon with respect to obstacles of the
     *             agent.
     */
    public getAgentTimeHorizonObst(agentNo: number) {
        return this.agents_[agentNo].timeHorizonObst_;
    }

    /**
     * \brief      Returns the two-dimensional linear velocity of a
     *             specified agent.
     * \param      agentNo         The number of the agent whose
     *                             two-dimensional linear velocity is to be
     *                             retrieved.
     * \return     The present two-dimensional linear velocity of the agent.
     */
    public getAgentVelocity(agentNo: number): Vector2 {
        return this.agents_[agentNo].velocity_;
    }

    /**
     * \brief      Returns the global time of the simulation.
     * \return     The present global time of the simulation (zero initially).
     */
    public getGlobalTime() {
        return this.time_;
    }

    public getNumAgents(): number {
        return this.agents_.length;
    }

    /**
     * \brief      Sets the maximum neighbor count of a specified agent.
     * \param      agentNo         The number of the agent whose maximum
     *                             neighbor count is to be modified.
     * \param      maxNeighbors    The replacement maximum neighbor count.
     */
    public setAgentMaxNeighbors(agentNo: number, maxNeighbors: number) {
        this.agents_[agentNo].maxNeighbors_ = maxNeighbors;
    }

    /**
     * \brief      Sets the maximum speed of a specified agent.
     * \param      agentNo         The number of the agent whose maximum speed
     *                             is to be modified.
     * \param      maxSpeed        The replacement maximum speed. Must be
     *                             non-negative.
     */
    public setAgentMaxSpeed(agentNo: number, maxSpeed: number) {
        this.agents_[agentNo].maxSpeed_ = maxSpeed;
    }

    /**
     * \brief      Sets the maximum neighbor distance of a specified agent.
     * \param      agentNo         The number of the agent whose maximum
     *                             neighbor distance is to be modified.
     * \param      neighborDist    The replacement maximum neighbor distance.
     *                             Must be non-negative.
     */
    public setAgentNeighborDist(agentNo: number, neighborDist: number) {
        this.agents_[agentNo].neighborDist_ = neighborDist;
    }

    /**
     * \brief      Sets the two-dimensional position of a specified agent.
     * \param      agentNo         The number of the agent whose
     *                             two-dimensional position is to be modified.
     * \param      position        The replacement of the two-dimensional
     *                             position.
     */
    public setAgentPosition(agentNo: number, position: Vector2) {
        this.agents_[agentNo].position_ = position;
    }
    /**
             * \brief      Sets the two-dimensional preferred velocity of a
             *             specified agent.
             * \param      agentNo         The number of the agent whose
             *                             two-dimensional preferred velocity is to be
             *                             modified.
             * \param      prefVelocity    The replacement of the two-dimensional
             *                             preferred velocity.
             */
    public setAgentPrefVelocity(agentNo: number, prefVelocity: Vector2) {
        this.agents_[agentNo].prefVelocity_ = prefVelocity;
    }

    public setAgentRadius(agentNo: number, radius: number) {
        this.agents_[agentNo].radius_ = radius;
    }

    /**
     * \brief      Sets the time horizon of a specified agent with respect
     *             to other agents.
     * \param      agentNo         The number of the agent whose time horizon
     *                             is to be modified.
     * \param      timeHorizon     The replacement time horizon with respect
     *                             to other agents. Must be positive.
     */
    public setAgentTimeHorizon(agentNo: number, timeHorizon: number) {
        this.agents_[agentNo].timeHorizon_ = timeHorizon;
    }

    /**
     * \brief      Sets the time horizon of a specified agent with respect
     *             to obstacles.
     * \param      agentNo         The number of the agent whose time horizon
     *                             with respect to obstacles is to be modified.
     * \param      timeHorizonObst The replacement time horizon with respect to
     *                             obstacles. Must be positive.
     */
    public setAgentTimeHorizonObst(agentNo: number, timeHorizonObst: number) {
        this.agents_[agentNo].timeHorizonObst_ = timeHorizonObst;
    }

    /**
     * \brief      Sets the two-dimensional linear velocity of a specified
     *             agent.
     * \param      agentNo         The number of the agent whose
     *                             two-dimensional linear velocity is to be
     *                             modified.
     * \param      velocity        The replacement two-dimensional linear
     *                             velocity.
     */
    public setAgentVelocity(agentNo: number, velocity: Vector2) {
        this.agents_[agentNo].velocity_ = velocity;
    }

    public clear() {
        this.agents_ = [];
        this.obstacles_ = [];
        this.time_ = 0;
        // this.defaultAgent_ = null;
        this.kdTree_.clear()
        this.timeStep_ = 1;
    }



    public processObstacles() {

        this.kdTree_.buildObstacleTree();
    }

    public queryVisibility(point1: Vector2, point2: Vector2, radius: number) {
        return this.kdTree_.queryVisibility(point1, point2, radius);
    }

    public setAgentDefaults(neighborDist: number, maxNeighbors: number, timeHorizon: number, timeHorizonObst: number, radius: number, maxSpeed: number, velocity: Vector2 = new Vector2(0, 0)): void {
        if (this.defaultAgent_ == null) {
            this.defaultAgent_ = new Agent();
        }

        this.defaultAgent_.maxNeighbors_ = maxNeighbors;
        this.defaultAgent_.maxSpeed_ = maxSpeed;
        this.defaultAgent_.neighborDist_ = neighborDist;
        this.defaultAgent_.radius_ = radius;
        this.defaultAgent_.timeHorizon_ = timeHorizon;
        this.defaultAgent_.timeHorizonObst_ = timeHorizonObst;
        this.defaultAgent_.velocity_ = velocity;
    }

    public setTimeStep(v: number): void {
        this.timeStep_ = v;
    }

    public getAgentOrientation(agentNo: number): number {

        return this.agents_[agentNo].orientation_;
    }

    public getOrca(agentNo: number): Line[] {

        return this.agents_[agentNo].orcaLines_;
    }

    public getAgentPositionScreen(agentNo: number): Vector2 {
        return this.agents_[agentNo].positionScreen_;
    }

}